#include "industrial_robot_control/core/application.h"
#include "industrial_robot_control/robot_controller.h"
#include "industrial_robot_control/config_loader.h"
#include "industrial_robot_control/utils/log.h"
#include "industrial_robot_control/utils/realtime_utils.h"

namespace IndustrialRobotControl {
namespace Core {

Application::Application(int argc, char**argv) 
    : argc_(argc), argv_(argv), running_(false) {
    // 初始化日志系统
    Utils::Log::initialize();
    
    // 加载配置
    config_loader_ = std::make_unique<ConfigLoader>();
    std::string config_path = (argc > 1) ? argv[1] : "/etc/robot_control/system.yaml";
    if (!config_loader_->load(config_path)) {
        throw std::runtime_error("Failed to load configuration file: " + config_path);
    }
    
    // 初始化控制器
    robot_controller_ = std::make_unique<RobotController>(config_loader_->get_config());
}

Application::~Application() {
    stop();
}

void Application::run() {
    LOG_INFO("Starting industrial robot control system v{}", PROJECT_VERSION);
    
    // 初始化控制器
    if (!robot_controller_->initialize()) {
        throw std::runtime_error("Failed to initialize robot controller");
    }
    
    // 设置实时线程
    Utils::RealtimeUtils::set_thread_realtime(pthread_self(), 90);
    
    running_ = true;
    main_loop_thread_ = std::thread(&Application::main_loop, this);
    
    // 等待线程结束
    main_loop_thread_.join();
}

void Application::stop() {
    if (running_) {
        LOG_INFO("Stopping industrial robot control system");
        running_ = false;
        if (main_loop_thread_.joinable()) {
            main_loop_thread_.join();
        }
        robot_controller_->shutdown();
    }
}

void Application::main_loop() {
    LOG_INFO("Entering main control loop");
    
    // 控制循环频率 (1000Hz)
    const auto loop_interval = std::chrono::microseconds(1000);
    
    while (running_) {
        const auto start_time = std::chrono::high_resolution_clock::now();
        
        // 执行控制周期
        robot_controller_->update();
        
        // 控制循环频率
        const auto end_time = std::chrono::high_resolution_clock::now();
        const auto elapsed = end_time - start_time;
        
        if (elapsed < loop_interval) {
            std::this_thread::sleep_for(loop_interval - elapsed);
        } else {
            LOG_WARN("Control loop overrun! Took {}us", 
                   std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count());
        }
    }
}

} // namespace Core
} // namespace IndustrialRobotControl
